#include "ProgramA.h"


bool ProgramA::execute()
{
  //init array  
  Coinbox * COINBOX = &Coinbox::Coinbox();
  Door * DOOR = &Door::Door();
  Motor * MOTOR = &Motor::Motor();
  Heater * HEATER = &Heater::Heater();
  Drain * DRAIN = &Drain::Drain();
  Soapbox1 * SOAPBOX1 = &Soapbox1::Soapbox1();
  Soapbox2 * SOAPBOX2 = &Soapbox2::Soapbox2();
  Sink * SINK = &Sink::Sink();


  //--------  config program A  ----------
  COINBOX->Start();
  DRAIN->Start();
  DOOR->Start();
  MOTOR->Start();
  HEATER->Start();
  SINK->Start();
  
  //check requirements
  COINBOX->waitForAmount(360);
  DOOR->checkDoor();
  //SOAPBOX1->checkIfFull();
  //SOAPBOX2->checkIfFull();
  
  //prewash
  DRAIN->setWaterLevel(50);
  MOTOR->Run(NORMALSPEED_R);
  delay(6000);
  MOTOR->Run(NORMALSPEED_L);
  delay(6000);
  MOTOR->Stop();
  delay(1000);
  SINK->drainWater();
  
  //main wash dash 1
  DRAIN->setWaterLevel(50);  
  HEATER->Heat(50); 
  for(int i = 0; i < 2; i++)
  {
    MOTOR->Run(NORMALSPEED_R);
    HEATER->KeepHeat(50,6000);
    MOTOR->Run(NORMALSPEED_L);
    HEATER->KeepHeat(50,6000);
  }
  MOTOR->Stop();
  delay(1000);
  SINK->drainWater();
  
  //main wash dash 2
  DRAIN->setWaterLevel(50);  
  for(int i = 0; i < 2; i++)
  {
    MOTOR->Run(NORMALSPEED_R);
    delay(6000);
    MOTOR->Run(NORMALSPEED_L);
    delay(6000);
  }
  MOTOR->Stop();  
  delay(1000);
  SINK->drainWater();  
  
  //main wash dash 3
  for(int i = 0; i < 2; i++)
  {
    MOTOR->Run(CENTRIFUGE_R);
    delay(3000);
    MOTOR->Run(CENTRIFUGE_L);
    delay(3000);
  }
  MOTOR->Stop();
  delay(1000);
  
  //successfully executed
  return 1;
}

void ProgramA::stop()
{
  COINBOX->Stop();
  DOOR->Stop();
  MOTOR->Stop();
  HEATER->Stop();
  DRAIN->Stop();
  SOAPBOX1->Stop();
  SOAPBOX2->Stop();
  SINK->Stop();
  
  delete COINBOX;
  delete DOOR;
  delete MOTOR;
  delete HEATER;
  delete DRAIN;
  delete SOAPBOX1;
  delete SOAPBOX2;
  delete SINK;
}
